#include "rtrobot.h"
#include "log/zuclog.h"
#include "motion/modbuscrc.h"
#include <math.h>
#include "string.h"
#include "rtdev/servo/jkservo.h"

#define ROBOT_DEBUG(fmt, ...) zuclog_debug("RTROBOT %d", fmt, robot_id_, ##__VA_ARGS__)
#define ROBOT_INFO(fmt, ...) zuclog_info("RTROBOT %d", fmt, robot_id_, ##__VA_ARGS__)
#define ROBOT_ERROR(fmt, ...) zuclog_error("RTROBOT %d", fmt, robot_id_, ##__VA_ARGS__)

template <typename T> static T get_data(uint8_t* data) { return data[6] + (data[5] << 8) + (data[4] << 16) + (data[3] << 24); }

rtdev::RtRobot::RtRobot(int i) : robot_id_(i)
{
    memset(&local_rtrobot_status_, 0, sizeof(local_rtrobot_status_));

    local_rtrobot_status_.dh.state = DHYN_NOT_USED;
    local_rtrobot_status_.dyn.state = DHYN_NOT_USED;
}

rtdev::RtRobot::~RtRobot() { ROBOT_INFO("RtRobot destroyed"); }

void rtdev::RtRobot::init() { ; }

void rtdev::RtRobot::link_servo(std::shared_ptr<rtdev::ServoItf>& servo, int i)
{
    if (!servo.get())
    {
        return;
    }
    servo->reset();  // 重新匹配参数
    joint_servo_.push_back(servo);
    ROBOT_INFO("Joint %d is bind on %d servo", i, servo->id());
}

// 每个rtbus周期调用
void rtdev::RtRobot::update()
{
    for (size_t i = 0; i < joint_servo_.size(); i++)
    {
        joint_servo_[i]->set_send_data(0);
        joint_servo_[i]->get_recv_data(0);
    }

    for (size_t i = 0; i < joint_servo_.size(); i++)
    {
        // 刷新本地储存的伺服数据
        local_rtrobot_status_.joint_status[i].is_alive = joint_servo_[i]->is_alive();
        local_rtrobot_status_.joint_status[i].enable = joint_servo_[i]->is_enable();

        local_rtrobot_status_.joint_status[i].act_pos = joint_servo_[i]->get_position(false);
        local_rtrobot_status_.joint_status[i].cmd_pos = joint_servo_[i]->get_position(true);
        local_rtrobot_status_.joint_status[i].act_torq = joint_servo_[i]->get_torq();
        local_rtrobot_status_.joint_status[i].act_collision_lv = joint_servo_[i]->get_act_collision_lv();
        local_rtrobot_status_.joint_status[i].torq_overload = joint_servo_[i]->is_tor_overload();

        local_rtrobot_status_.joint_status[i].statusword = joint_servo_[i]->get_statusword();

        // rtrobot_status_.joint_status[i].errorcode = joint_servo_[i]->stat_.errorcode;
        // rtrobot_status_.joint_status[i].opmode_display = joint_servo_[i]->stat_.opmode_display;
    }
    check_alive();
}

void rtdev::RtRobot::check_alive()
{
    ActiveStatus last_status = local_rtrobot_status_.active_status;
    bool is_active = joint_servo_.size() > 0 ? true : false;
    local_rtrobot_status_.active_status.is_alive = false;
    for (size_t i = 0; i < joint_servo_.size(); i++)
    {
        if (joint_servo_[i]->is_alive())
        {
            local_rtrobot_status_.active_status.is_alive = true;
        }
        else
        {
            is_active = false;
        }
    }
    local_rtrobot_status_.active_status.is_active = is_active;
    if (last_status.is_active != local_rtrobot_status_.active_status.is_active || last_status.is_alive != local_rtrobot_status_.active_status.is_alive)
    {
        ROBOT_INFO("robot serial %d is %s and %s",
                   local_rtrobot_status_.serial_num_,
                   local_rtrobot_status_.active_status.is_active ? "active" : "inactive",
                   local_rtrobot_status_.active_status.is_alive ? "alive" : "dead");
    }
}

// 每个rtbus周期调用，在rtbus线程中调用
void rtdev::RtRobot::prepare_status_for_motion()
{
    std::lock_guard<std::mutex> l(status_mutex_);
    rtrobot_status_fdb_ = local_rtrobot_status_;
}

// 每个traj周期调用，在motion线程中被调用
void rtdev::RtRobot::get_rtrobot_status_for_motion(RtRobotStatus* status)
{
    std::lock_guard<std::mutex> l(status_mutex_);
    *status = local_rtrobot_status_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////
int rtdev::RtRobotMgr::add_rtrobot(std::shared_ptr<rtdev::RtRobot> robot)
{
    rtrobots_.emplace(rtrobots_.size(), robot);
    return 0;
}
int rtdev::RtRobotMgr::del_rtrobot(uint8_t group_id)
{
    if (rtrobots_.count(group_id) == 0)
    {
        return -1;
    }
    rtrobots_.erase(group_id);
    return 0;
}
std::shared_ptr<rtdev::RtRobot> rtdev::RtRobotMgr::get_rtrobot(uint8_t group_id)
{
    if (rtrobots_.count(group_id) == 0)
    {
        return nullptr;
    }
    return rtrobots_[group_id];
}
void rtdev::RtRobotMgr::reset() { rtrobots_.clear(); }

size_t rtdev::RtRobotMgr::robot_num() { return rtrobots_.size(); }